import rclpy
from rclpy.node import Node
from gobang_interfaces.msg import SerialChess
import serial
import struct
import numpy as np

class ChessboardInterpolator:
    """
    19x19棋盘的双线性插值器
    用于根据四个角点坐标计算棋盘上任意位置的坐标值
    """
    
    def __init__(self, corner_points):
        """
        初始化插值器
        
        Args:
            corner_points: 字典，包含四个角点的坐标
                格式: {
                    'top_left': (x, y),     # (0, 0)
                    'top_right': (x, y),    # (0, 18)
                    'bottom_left': (x, y),  # (18, 0)
                    'bottom_right': (x, y)  # (18, 18)
                }
        """
        self.corners = corner_points
        self.grid_size = 14  # 19x19棋盘，索引从0到18
        
        # 提取角点坐标
        self.top_left = np.array(corner_points['top_left'])
        self.top_right = np.array(corner_points['top_right'])
        self.bottom_left = np.array(corner_points['bottom_left'])
        self.bottom_right = np.array(corner_points['bottom_right'])
        
    def bilinear_interpolation(self, row, col):
        """
        双线性插值计算指定棋盘位置的坐标
        
        Args:
            row: 行索引 (0-18)
            col: 列索引 (0-18)
            
        Returns:
            tuple: (x, y) 插值后的坐标
        """
        # 归一化坐标 (0-1范围)
        u = col / self.grid_size  # 列方向插值参数
        v = row / self.grid_size  # 行方向插值参数
        
        # 双线性插值公式
        # P(u,v) = (1-u)(1-v)P00 + u(1-v)P10 + (1-u)vP01 + uvP11
        point = (
            (1 - u) * (1 - v) * self.top_left +      # P00: 左上角
            u * (1 - v) * self.top_right +           # P10: 右上角
            (1 - u) * v * self.bottom_left +         # P01: 左下角
            u * v * self.bottom_right                 # P11: 右下角
        )
        
        return tuple(point)
    
    def get_coordinate(self, chess_pos):
        """
        获取棋盘位置对应的坐标
        
        Args:
            chess_pos: 元组 (row, col) 棋盘位置
            
        Returns:
            tuple: (x, y) 插值后的坐标
        """
        row, col = chess_pos
        
        # 检查输入范围
        if not (0 <= row <= 18 and 0 <= col <= 18):
            raise ValueError(f"棋盘位置超出范围: ({row}, {col}), 应在 (0,0) 到 (18,18) 之间")
        
        return self.bilinear_interpolation(row, col)
    
    def get_multiple_coordinates(self, positions):
        """
        批量获取多个位置的坐标
        
        Args:
            positions: 位置列表 [(row1, col1), (row2, col2), ...]
            
        Returns:
            list: 坐标列表 [(x1, y1), (x2, y2), ...]
        """
        return [self.get_coordinate(pos) for pos in positions]
    
    def verify_corners(self):
        """
        验证角点插值是否正确
        """
        print("=== 角点验证 ===")
        test_cases = [
            ((0, 0), self.top_left, "左上角"),
            ((0, 18), self.top_right, "右上角"), 
            ((18, 0), self.bottom_left, "左下角"),
            ((18, 18), self.bottom_right, "右下角")
        ]
        
        for pos, expected, name in test_cases:
            calculated = self.get_coordinate(pos)
            print(f"{name} {pos}: 期望 {expected}, 计算 {calculated}")
            print(f"误差: {np.linalg.norm(np.array(calculated) - expected):.6f}")
        print()

# 简单使用示例
def quick_interpolate(row, col):
    """
    快速插值函数 - 直接使用预定义的角点
    
    Args:
        row: 行坐标 (0-18)
        col: 列坐标 (0-18)
        
    Returns:
        tuple: (x, y) 插值后的坐标
    """
    corner_points = {
        # 'top_left': (203, 140),      # (0, 0) 左上角
        # 'top_right': (211, 568),     # (0, 18) 右上角
        # 'bottom_left': (-195, 143),  # (18, 0) 左下角  
        # 'bottom_right': (-195, 571)  # (18, 18) 右下角
        'top_left': (173, 524),      # (0, 0) 左上角
        'top_right': (-145, 528),     # (0, 18) 右上角
        'bottom_left': (164, 190),  # (18, 0) 左下角  
        'bottom_right': (-153, 193)  # (18, 18) 右下角
    }
    
    interpolator = ChessboardInterpolator(corner_points)
    return interpolator.get_coordinate((row, col))

# 串口配置（根据实际设备修改）
SERIAL_PORT = '/dev/ttyS3'  # 串口号
BAUD_RATE = 9600      # 波特率

# 协议常量
START_BYTES = b'\xFE\x01'  # 起始位
END_BYTES = b'\xEF\x10'    # 结束位

def calculate_checksum(command:int, data:list)->int:
    """计算校验和：指令位 + 所有数据字节之和,保留低2字节"""
    total = command
    for byte in data:
        total += byte
    return total & 0xFFFF  # 取低16位

class SerialNode(Node):

    def __init__(self):
        super().__init__('serial_node_sub')
        self.ser = serial.Serial(SERIAL_PORT, BAUD_RATE)
        self.get_logger().info(f"监听串口 {SERIAL_PORT}，波特率 {BAUD_RATE}...")
        self.serial_sub = self.create_subscription(SerialChess,'serial_chess_pub', self.serial_callback, 10)

    def serial_callback(self, serial_msg: SerialChess):
        """处理订阅的消息（如果需要）"""
        self.get_logger().info(f"收到串口数据, 落子:({serial_msg.row}, { serial_msg.col})")
        position = quick_interpolate(serial_msg.row, serial_msg.col)
        self.get_logger().info(f"计算位置坐标: {position}")
        # 发送到串口
        if self.ser.is_open:
            # try:
                # 组装数据包
                packet = (
                    START_BYTES +
                    struct.pack('B', 0xA1) +  # 将uint8转换为bytes
                    struct.pack('>h', int(position[0])) + # x坐标
                    struct.pack('>h', int(position[1])) + # y坐标
                    struct.pack('>h', calculate_checksum(0xA1, [int(position[0]),int(position[1])])) +
                    END_BYTES
                )
                self.ser.write(packet)
                self.get_logger().info(f"发送数据包：{packet.hex(' ')}")
            # except Exception as e:
            #     self.get_logger().error(f"发送错误：{str(e)}")
        
def main(args=None):
    rclpy.init(args=args)
    serial_node = SerialNode()
    rclpy.spin(serial_node)
    serial_node.destroy_node()
    rclpy.shutdown()